#include "Q_to_Q.h"

/*四元数的逆：+q0,-q1,-q2,-q3*/

void quaternion_inverse(Q_info_Typedef *q_in , Q_info_Typedef *q_out) {
	
    float norm_sq = q_in->q0 * q_in->q0 + q_in->q1 * q_in->q1 + q_in->q2 * q_in->q2 + q_in->q3 * q_in->q3;
    if (norm_sq > 0.0) {
        q_out->q0 = +q_in->q0 / norm_sq;
        q_out->q1 = -q_in->q1 / norm_sq;
        q_out->q2 = -q_in->q2 / norm_sq;
        q_out->q3 = -q_in->q3 / norm_sq;
    } else {
        q_out->q0 = 0.0f;
		q_out->q1 = 0.0f;
		q_out->q2 = 0.0f;
		q_out->q3 = 0.0f;
    }
}

/*四元数乘法：Q1*P2
 =[+q0 -q1 -q2 -q3    [+p0
   +q1 -q0 -q3 +q2  *  +p1
   +q2 +q3 +q0 -q1     +p2
   +q3 -q2 +q1 +q0]    +p3]
 =[+p0 -p1 -p2 -p3    [+q0
   +p1 -p0 -p3 +p2  *  +q1
   +p2 +p3 +p0 -p1     +q2
   +p3 -p2 +p1 +p0]    +q3]
 */

void quaternion_multiply(Q_info_Typedef *q1, Q_info_Typedef *q2, Q_info_Typedef *q3) {

    q3->q0 = q1->q0 * q2->q0 - q1->q1 * q2->q1 - q1->q2 * q2->q2 - q1->q3 * q2->q3;
    q3->q1 = q1->q0 * q2->q1 + q1->q1 * q2->q0 + q1->q2 * q2->q3 - q1->q3 * q2->q2;
    q3->q2 = q1->q0 * q2->q2 - q1->q1 * q2->q3 + q1->q2 * q2->q0 + q1->q3 * q2->q1;
    q3->q3 = q1->q0 * q2->q3 + q1->q1 * q2->q2 - q1->q2 * q2->q1 + q1->q3 * q2->q0;
}
